﻿/*
 * ROSVEM Project
 *    Coded by Robert Jacobs 2010-2011
 *    http://robertjacobs.eu
 *    
 * This code is released under the Apache 2.0 license, copyright Robert Jacobs 2010-2011
 * */

using System;
using System.Text;
using System.IO.Ports;
using System.Threading;

using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

using GHIElectronics.NETMF.FEZ;
using GHIElectronics.NETMF.Hardware;

namespace Robot
{
    public class Program
    {
        //Serial connection between robot and control centre
        private static SerialPort ControlCentre;

        //Send and recieve bytes for the serial data
        private static byte[] rxData;
        private static byte[] txData;

        //Motor 1 variables
        private static PWM _pwm1;
        private static string _dirM1;
        private static sbyte speed;
        private static OutputPort _dir1;

        //Motor 2 variables
        private static PWM _pwm2;
        private static string _dirM2;
        private static OutputPort _dir2;

        //Data received from the serial connection
        private static string dataReceived;

        //Split version of the dataReveived
        private static string[] dataLines;

        private static AnalogIn BatteryVoltage;
        /// <summary>
        /// Main method. Initialises everything
        /// </summary>
        public static void Main()
        {
            ControlCentre = new SerialPort("COM1",115200);

            ControlCentre.DataReceived += ControlCentre_DataReceived;
            ControlCentre.Open();

            _pwm1 = new PWM((PWM.Pin)FEZ_Pin.PWM.Di8);
            _dir1 = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di9, false);

            _pwm2 = new PWM((PWM.Pin)FEZ_Pin.PWM.Di6);
            _dir2 = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di7, false);

            BatteryVoltage = new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An0);
            BatteryVoltage.SetLinearScale(0, 3300);

            Thread sendValuesToCommandCenter = new Thread(SendData);
            sendValuesToCommandCenter.Start();

            Debug.EnableGCMessages(false);

            Thread.Sleep(Timeout.Infinite);
        }

        /// <summary>
        /// Gets triggered when we receive data over the serial connection
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        static void ControlCentre_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            rxData = new byte[ControlCentre.BytesToRead];
            ControlCentre.Read(rxData, 0, rxData.Length);

            dataReceived = new String(Encoding.UTF8.GetChars(rxData));

            Debug.Print(dataReceived);

            dataLines = dataReceived.Split(';');

            if (dataLines.Length == 4)
            {
                _dirM1 = dataLines[0];
                _dirM2 = dataLines[2];

                switch (_dirM1)
                {
                    case "L":
                        _dir1.Write(true);
                        break;
                    case "R":
                        _dir1.Write(false);
                        break;
                }

                switch (_dirM2)
                {
                    case "L":
                        _dir2.Write(true);
                        break;
                    case "R":
                        _dir2.Write(false);
                        break;
                }

                speed = Convert.ToSByte(dataLines[1]);
                if(speed >95 && speed<100)
                {
                    speed = 100;
                }

                _pwm1.Set(1000, (byte) (100 - System.Math.Abs(speed)));
                _pwm2.Set(1000, (byte) (100 - System.Math.Abs(speed)));
            }

            rxData = null;
            dataReceived = null;
            dataLines = null;
        }

        /// <summary>
        /// Send data over serial to the control centre
        /// </summary>
        public static void SendData()
        {
            Random random = new Random();
            while (true)
            {
                double voltage = ReadBatteryVoltage();
                string sendData = "V+: "+voltage+"V;Amp1: "+random.Next(20)+"A;Amp2: "+random.Next(20)+"A; Temp: "+random.Next(40)+"C";

                txData = new byte[sendData.Length];
                txData = Encoding.UTF8.GetBytes(sendData);
                ControlCentre.Write(txData, 0, txData.Length);

                ReadBatteryVoltage();
                Thread.Sleep(100);
            }
        }

        public static double ReadBatteryVoltage()
        {
            double analogVoltage = BatteryVoltage.Read();
            return ((analogVoltage / 1000) * 7.6600985221674876847290640394089);
        }
    }
}
